#include "methods.h"

#include <getopt.h>
#include <iostream>
#include <tuple>
#include <fstream>
#include <cmath>
#include <queue>
#include <algorithm>

#include <public/utils.h>
#include <graph.priv.h>
#include <config-fnt.h>

void planarize(Graph gr) {
	std::vector<double> edgeLens(gr.edges().size(), 0);
	std::vector<double> edgeLensMax(gr.edges().size(), 0);
	for(auto e: gr.edges()) {
		double d=0;
		for(size_t i=1; i<e.points().size(); i++) {
			d+=e.points()[i].distTo(e.points()[i-1]);
		}
		edgeLens[e.index()]=d;
	}
	std::vector<size_t> updown{};
	{
		std::queue<Vertex> nodes{};
		for(auto n: gr.trees())
			nodes.push(n.root());
		while(!nodes.empty()) {
			auto n=nodes.front();
			nodes.pop();
			for(auto& ep: n.neighbors()) {
				if(ep.first.parentVertex()==n) {
					updown.push_back(ep.first.index());
					if(ep.first.parentVertex()==ep.first.leftVertex()) {
						nodes.push(ep.first.rightVertex());
					} else {
						nodes.push(ep.first.leftVertex());
					}
				}
			}
		}
	}
	std::vector<size_t> downup{};
	for(size_t i=updown.size(); i-->0;)
		downup.push_back(updown[i]);
	for(size_t i=0; i<downup.size(); i++) {
		auto idx=downup[i];
		auto l=edgeLensMax[idx]+=edgeLens[idx];
		auto pe=gr.edges()[idx].parentEdge();
		if(pe) {
			if(edgeLensMax[pe.index()]<l)
				edgeLensMax[pe.index()]=l;
		}
	}

	std::vector<double> edgeAngles(gr.edges().size());
	std::vector<std::pair<double, double>> vertPos(gr.vertices().size());

	FILE* f=fopen("./planarized.mp", "w");
	for(size_t ni=0; ni<gr.trees().size(); ni++) {
		auto n=gr.trees()[ni];
		std::queue<Vertex> nodes;
		nodes.push(n.root());
		fprintf(f, "beginfig(%zd);\n", ni);
		fprintf(f, "pickup pencircle scaled .01cm;\n");
		fprintf(f, "pickup pencircle scaled .15cm;\n");
		fprintf(f, "label.lft(\"%s\", (0, 0cm));\n", n.name().c_str());
		fprintf(f, "drawdot (%lfcm,%lfcm) withcolor black;\n", 0.0, 0.0);
		vertPos[n.root().index()]={0.0, 0.0};

		while(!nodes.empty()) {
			auto v=nodes.front();
			nodes.pop();
			double prevDir=0.0;
			if(v.parentEdge())
				prevDir=edgeAngles[v.parentEdge().index()];
			auto prevPos=vertPos[v.index()];

			std::vector<size_t> edgs{};
			for(auto& ep: v.neighbors()) {
				if(ep.first.parentVertex()==v) {
					edgs.push_back(ep.first.index());
					if(ep.first.parentVertex()==ep.first.leftVertex()) {
						nodes.push(ep.first.rightVertex());
					} else {
						nodes.push(ep.first.leftVertex());
					}
				}
			}
			std::sort(edgs.begin(), edgs.end(), [&edgeLensMax](size_t a, size_t b) -> bool { return edgeLensMax[a]>edgeLensMax[b]; });
			for(size_t j=0; j<edgs.size(); j++) {
				double dir=prevDir+j*.5;

				auto posNew=prevPos;
				auto edg=edgs[j];
				edgeAngles[edg]=dir;
				posNew.first+=edgeLens[edg]*cos(dir)/1000;
				posNew.second+=edgeLens[edg]*sin(dir)/1000;
					if(gr.edges()[edg].parentVertex()==gr.edges()[edg].leftVertex()) {
						vertPos[gr.edges()[edg].rightVertex().index()]=posNew;
					} else {
						vertPos[gr.edges()[edg].leftVertex().index()]=posNew;
					}
				fprintf(f, "pickup pencircle scaled .01cm;\n");
				fprintf(f, "draw ((%lfcm,%lfcm)--(%lfcm,%lfcm)) withcolor green;\n", prevPos.first, prevPos.second, posNew.first, posNew.second);
			}
		}
		fprintf(f, "endfig;\n");

	}
	fclose(f);
}
